
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "LR1.h"
#include "serializationUtils.h"

int avanzarDistanciaCM(uint8_t vel, unsigned short int dist, char * id,
        dest_list_t *list) {

    command_arg_t com;
    commandAttrInit(&com, 1, 1, 0, 0, 0);

    com.command = FWD_DIST;

    com.byte_args[0] = vel;

    com.short_args[0] = dist;

    com.destination = id;
    com.destination_list = list;

    if (sendCommand(com) == -1) {
#ifdef LR1_DEBUG
        perror("com_send");
#endif
        return -1;
    }

//    struct msg_t msg;
//    if (createMsg(com, &msg) == -1) {
//#ifdef LR1_DEBUG
//        perror("toMsg");
//#endif
//        return -1;
//    }
//
//    if (msgOutput(msg, com.destination_list)<0){
//#ifdef LR1_DEBUG
//        perror("msgOutput");
//#endif
//        return -1;
//    }


    return (0);

}

int avanzarPulsos(uint8_t vel, short int pulsos, char * id, dest_list_t *list) {

    command_arg_t com;

    commandAttrInit(&com, 1, 1, 0, 0, 0);

    com.command = FWD_PULSE;

    com.byte_args[0] = vel;

    com.short_args[0] = pulsos;

    com.destination = id;
    com.destination_list = list;

    if (sendCommand(com) == -1) {
#ifdef LR1_DEBUG
        perror("com_send");
#endif
        return -1;
    }

//    struct msg_t msg;
//    if (createMsg(com, &msg) == -1) {
//#ifdef LR1_DEBUG
//        perror("toMsg");
//#endif
//        return -1;
//    }
//
//    if (msgOutput(msg, com.destination_list)<0){
//#ifdef LR1_DEBUG
//        perror("msgOutput");
//#endif
//        return -1;
//    }

    return (0);

}

int girarGrados(uint8_t dir, uint8_t vel, short int grados, char *id,
        dest_list_t *list) {

    command_arg_t com;

    commandAttrInit(&com, 1, 1, 0, 0, 0);

    com.command = dir;

    com.byte_args[0] = vel;

    if (dir == RIGHT_DEG) {
        com.short_args[0] = -grados;
    } else if (dir == LEFT_DEG) {
        com.short_args[0] = grados;
    }

    com.destination = id;
    com.destination_list = list;

    if (sendCommand(com) == -1) {
#ifdef LR1_DEBUG
        perror("com_send");
#endif
        return -1;
    }

//    struct msg_t msg;
//    if (createMsg(com, &msg) == -1) {
//#ifdef LR1_DEBUG
//        perror("toMsg");
//#endif
//        return -1;
//    }
//
//    if (msgOutput(msg, com.destination_list)<0){
//#ifdef LR1_DEBUG
//        perror("msgOutput");
//#endif
//        return -1;
//    }

    return (0);

}

int girarRadianes(uint8_t dir, uint8_t vel, float radianes, char *id,
        dest_list_t *list) {

    command_arg_t com;

    commandAttrInit(&com, 1, 0, 0, 1, 0);

    com.command = dir;

    com.byte_args[0] = vel;

    if (dir == RIGHT_RAD) {
        com.float_args[0] = -(radianes);
    } else if (dir == LEFT_RAD) {
        com.float_args[0] = radianes;
    }

    com.destination = id;
    com.destination_list = list;

    if (sendCommand(com) == -1) {
#ifdef LR1_DEBUG
        perror("com_send");
#endif
        return -1;
    }

//    struct msg_t msg;
//    if (createMsg(com, &msg) == -1) {
//#ifdef LR1_DEBUG
//        perror("toMsg");
//#endif
//        return -1;
//    }
//
//    if (msgOutput(msg, com.destination_list)<0){
//#ifdef LR1_DEBUG
//        perror("msgOutput");
//#endif
//        return -1;
//    }

    return 0;

}

int avanzar(uint8_t vel, char *id, dest_list_t *list) {

    command_arg_t com;

    commandAttrInit(&com, 1, 0, 0, 0, 0);

    com.command = FWD;

    com.byte_args[0] = vel;

    com.destination = id;
    com.destination_list = list;

    if (sendCommand(com) == -1) {
#ifdef LR1_DEBUG
        perror("com_send");
#endif
        return -1;
    }
//    struct msg_t msg;
//    if (createMsg(com, &msg) == -1) {
//#ifdef LR1_DEBUG
//        perror("toMsg");
//#endif
//        return -1;
//    }
//
//    if (msgOutput(msg, com.destination_list)<0){
//#ifdef LR1_DEBUG
//        perror("msgOutput");
//#endif
//        return -1;
//    }

    return 0;

}

int retroceder(uint8_t vel, char *id, dest_list_t *list) {

    command_arg_t com;

    commandAttrInit(&com, 1, 0, 0, 0, 0);

    com.command = BWD;

    com.byte_args[0] = vel;

    com.destination = id;
    com.destination_list = list;

    if (sendCommand(com) == -1) {
#ifdef LR1_DEBUG
        perror("com_send");
#endif
        return -1;
    }
//    struct msg_t msg;
//    if (createMsg(com, &msg) == -1) {
//#ifdef LR1_DEBUG
//        perror("toMsg");
//#endif
//        return -1;
//    }
//
//    if (msgOutput(msg, com.destination_list)<0){
//#ifdef LR1_DEBUG
//        perror("msgOutput");
//#endif
//        return -1;
//    }

    return 0;

}

int detenerse(char *id, dest_list_t *list) {

    command_arg_t com;

    commandAttrInit(&com, 0, 0, 0, 0, 0);

    com.command = STOP;

    com.destination = id;
    com.destination_list = list;

    if (sendCommand(com) == -1) {
#ifdef LR1_DEBUG
        perror("com_send");
#endif
        return -1;
    }
//    struct msg_t msg;
//    if (createMsg(com, &msg) == -1) {
//#ifdef LR1_DEBUG
//        perror("toMsg");
//#endif
//        return -1;
//    }
//
//    if (msgOutput(msg, com.destination_list)<0){
//#ifdef LR1_DEBUG
//        perror("msgOutput");
//#endif
//        return -1;
//    }

    return 0;

}

uint8_t* barridoUS(char *id, dest_list_t *list) {

    command_arg_t com;

    commandAttrInit(&com, 0, 0, 0, 0, 0);

    com.command = SWEEP;

    com.destination = id;
    com.destination_list = list;

    com.opt|=RESPONSE_REQUIRED_BIT;

    if (sendCommand(com) == -1) {
#ifdef LR1_DEBUG
        perror("com_send");
#endif
        return NULL;
    }
//    struct msg_t msg;
//    if (createMsg(com, &msg) == -1) {
//#ifdef LR1_DEBUG
//        perror("toMsg");
//#endif
//        return NULL;
//    }
//
//    if (msgOutput(msg, com.destination_list)<0){
//#ifdef LR1_DEBUG
//        perror("msgOutput");
//#endif
//        return NULL;
//    }

    uint8_t *ret = NULL;
    ret = getResponse();
    if (ret == NULL) {
#ifdef LR1_DEBUG
        perror("get_response");
#endif
    }

    return ret;

}

short int sensorUSobtenerMedidaCM(char *id, dest_list_t *list) {

    command_arg_t com;

    commandAttrInit(&com, 0, 0, 0, 0, 0);

    com.command = READ;

    com.destination = id;
    com.destination_list = list;

    com.opt|=RESPONSE_REQUIRED_BIT;

    if (sendCommand(com) == -1) {
#ifdef LR1_DEBUG
        perror("com_send");
#endif
        return -1;
    }

    uint8_t *ret = NULL;
    ret = getResponse();
    if (ret == NULL) {
#ifdef LR1_DEBUG
        perror("get_response");
#endif
        return -1;
    } else {
        unsigned short int val = deSerializeShort(ret);
        printf("\nobtenerUS %d\n", val);
        return val;
    }

}

int foo_cmd(uint8_t cmd, int arg1, int arg2, int arg3, int arg4, char *id,
        dest_list_t *list) {

    command_arg_t com;

    commandAttrInit(&com, 0, 0, 4, 0, 0);

    com.command = cmd;

    com.int_args[0] = arg1;
    com.int_args[1] = arg2;
    com.int_args[2] = arg3;
    com.int_args[3] = arg4;

    com.destination = id;
    com.destination_list = list;

//    if (createMsg(com) == -1) {
//#ifdef LR1_DEBUG
//        perror("create_msg");
//#endif
//        return -1;
//    }

    return 0;

}

int foo_dbl(uint8_t cmd, int arg1, int arg2, double arg3, char *id,
        dest_list_t *list) {

    command_arg_t com;

    commandAttrInit(&com, 0, 0, 2, 0, 1);

    com.command = cmd;

    com.int_args[0] = arg1;
    com.int_args[1] = arg2;

    com.dbl_args[0] = arg3;

    com.destination = id;
    com.destination_list = list;

//    if (createMsg(com) == -1) {
//#ifdef LR1_DEBUG
//        perror("create_msg");
//#endif
//        return -1;
//    }

    return 0;

}

void comander() {

    char input[20];
    char c[1];
    char prev;
    char buf[50][20];
    char *id = input;
    dest_list_t *a = NULL;
    dest_list_t b;
    uint8_t i = 0;

    printf("\nWho? (o 'b' para broadcast, 'm' para multicast): ");
    scanf("%s", input);

    if (strcmp(input, "m") == 0) {

        i = 0;
        printf("\nId de cada integrante del grupo (0 para finalizar).");

        do {

            printf("\nId %d: ", i);
            scanf("%s", buf[i]);

        } while (buf[i++][0] != '0');

        b.size = i - 1;
        id = NULL;
        a = &b;
    } else if (strcmp(input, "b") == 0) {
        strcpy(id, "broadcast");
    }

    char *d[b.size];

    if (a != NULL) {

        for (i = 0; i < b.size; i++) {
            d[i] = malloc(strlen(buf[i]) * sizeof(char));
            strcpy(d[i], buf[i]);
        }

        b.id_list = d;
    }

    console_io_open();
    printf("\nESC para salir.\n ");
    //-- Main loop
    do {
        //-- if there is a key pressed by the user
        if (console_io_kbhit()) {

            //-- Read the character
            c[0] = console_io_getch();

            //-- Send the character to the serial port, if different from ESC
            //if (c[0]!=ESC && c[0]!=prev) {
            if (c[0] != ESC) {

                switch (c[0]) {

                case FWD: {
                    avanzar(80, id, a);
                    break;
                }
                case FWD_DIST: {
                    avanzarDistanciaCM(80, 40, id, a);
                    break;
                }
                case FWD_PULSE: {
                    avanzarPulsos(80, 40, id, a);
                    break;
                }
                case RIGHT_DEG: {
                    girarGrados(RIGHT_DEG, 60, 30, id, a);
                    break;
                }
                case LEFT_DEG: {
                    girarGrados(LEFT_DEG, 60, 30, id, a);
                    break;
                }
                case RIGHT_RAD: {
                    girarRadianes(RIGHT_RAD, 40, 0.786000, id, a);
                    break;
                }
                case LEFT_RAD: {
                    girarRadianes(LEFT_RAD, 40, 0.786000, id, a);
                    break;
                }
                case BWD: {
                    retroceder(80, id, a);
                    break;
                }
                case STOP: {
                    detenerse(id, a);
                    break;
                }
                case SWEEP: {
                    uint8_t *ret = barridoUS(id, a);
                    if (ret != NULL) {
                        printf("\n\nlectura: ");
                        for (i = 0; i < 10; i++) {
                            printf("%d ", deSerializeShort(&ret[2 * i]));
                        }
                        printf("\n");
                    }
                    break;
                }
                case READ: {
                    int16_t ret = 0;
                    ret=sensorUSobtenerMedidaCM(id, a);
                    if (ret > 0 ) {
                        printf("\n\nlectura: %d\n",ret);
                    }
                    break;
                }

                case 0x75:
                    updateNodeList();
                    break;

                default:
                    break;
                }
                prev = c[0];
                //usleep(100000);
            }
        }
    } while (c[0] != ESC);

    console_io_close();

    if (a != NULL) {
        for (i = 0; i < b.size; i++) {
            free(d[i]);
        }
    }
}

int wander () {

    char id[20];
    char c[1];

    printf("\nWander Who? : ");
    scanf("%s", id);
    short int angulo = 0;
    srand(time(NULL));

    uint8_t velocidad = 40;
    uint8_t velGiro = 40;

    console_io_open();
    printf("\nESC para salir.\n ");
    //-- Main loop

        //detenerse(id, NULL);
        //usleep(199000);
    do {
        //-- if there is a key pressed by the user
        if (console_io_kbhit()) {
            c[0] = console_io_getch();
        }

        if (sensorUSobtenerMedidaCM(id, NULL) < 30) {

            angulo = (short int) (rand() % (359 + 1 - 2)) + 2;
            girarGrados(LEFT_DEG, velGiro, angulo, id, NULL);
            usleep(599000);
        } else {
            avanzarDistanciaCM(velocidad, 10, id, NULL);
            usleep(599000);

        }

    } while (c[0] != ESC);

    detenerse(id, NULL);

    console_io_close();

    return 0;

}





